using System;
using System.Threading;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using SecretLabs.NETMF.Hardware.Netduino;

using mfLogger;

using OST.Shared;
using OST.Shared.Interfaces;
using OST.Shared.Extensions;
using OST.Shared.SharedTypes;
using OST.Persistence;

namespace OST.Motor
{
    /// <summary>
    /// endstop-sensor <see cref="IMotor"/> implementation
    /// </summary>
    public class EndStopSensorMotor : IMotor
    {
        #region Fields
        OutputPort _swDir = new OutputPort(Pins.GPIO_PIN_D0, false);//true --> MovingUp; false --> MovingDown;
        OutputPort _swDc = new OutputPort(Pins.GPIO_PIN_D1, false);

        InterruptPort _endDown = new InterruptPort(Pins.GPIO_PIN_D8, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeHigh);
        InterruptPort _endUp = new InterruptPort(Pins.GPIO_PIN_D9, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeHigh);


        Stopwatch _stopwatch = new Stopwatch();
        enumEndSensorType _suspendedSensor = enumEndSensorType.None;

        Timer _timer4Motor = null;

        int _timeWayCoefficient = int.MinValue;
        int _stopCounter = 0;
        int _currentPosition = 0;

        #endregion

        #region CTOR
        public EndStopSensorMotor()
        {
            Logger.Warning(GetType().Name + " implementation is experimental");
        }

        #endregion

        #region Methods

        private void UpdateMotorPosition()
        {
            var motorruntimeinMS = _stopwatch.RuntimeInMilliseconds - MotorConfig.Preload;
            Position = MotorCalculator.CalculateMotorPosition(Position
                                                        , motorruntimeinMS
                                                        , DrivingDirection
                                                        , _timeWayCoefficient
                                                        , FrameConfig.Stroke
                                                        , MotorConfig.OverrunThr
                                                        , MotorConfig.Overrun
                                                        , _stopCounter);

            _stopwatch.Reset();
            PrintEngineState();
        }

        private void PrintEngineState()
        {
            Logger.Debug("EngineState = " + enumMotorDrivingDirectionTranslator.Translate(_drivingDirection)
                + " | LastDuration = " + _stopwatch.LastRuntime + " (" + _stopwatch.RuntimeInMilliseconds + " ms)"
                + " | ArmPosition = " + Position + " mm"
                + " | Stops = " + _stopCounter);

        }

        private void endUp_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            //avoid double invocation
            if (_drivingDirection != enumMotorDrivingDirection.MovingUp)
            {
                return;
            }
            Logger.Debug("endUp_OnInterrupt() received");

            DrivingDirection = enumMotorDrivingDirection.EndUp;
            _swDc.Write(false);
            MotorDriving = false;
            _stopCounter = 0;
            _currentPosition = FrameConfig.Stroke;

            SuspendEndSensorsAsync();
        }

        private void endDown_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            //avoid double invocation 
            if (_drivingDirection != enumMotorDrivingDirection.MovingDown)
            {
                return;
            }
            Logger.Debug("endDown_OnInterrupt() received");

            DrivingDirection = enumMotorDrivingDirection.EndDown;
            _swDc.Write(false);
            MotorDriving = false;
            _stopCounter = 0;
            _currentPosition = 0;

            SuspendEndSensorsAsync();
        }

        private void SuspendEndSensorsAsync()
        {
            var thread = new Thread(new ThreadStart(SuspendEndSensors));
            thread.Start();
        }

        private void SuspendEndSensors()
        {
            Logger.Debug("SuspendEndSensors() executed, with " + enumEndSensorTypeTranslator.Translate(_suspendedSensor) + " suspended");

            SuspendEndDown();
            SuspendEndUp();
            Thread.Sleep(MotorConfig.Delay);

            if (_suspendedSensor == enumEndSensorType.EndDown)//activate just the upper one
                ResumeEndUp();
            else if (_suspendedSensor == enumEndSensorType.EndUp)//activate just the lower one
                ResumeEndDown();
        }

        private void ResumeEndUp()
        {
            _endUp.OnInterrupt += endUp_OnInterrupt;
        }

        private void ResumeEndDown()
        {
            _endDown.OnInterrupt += endDown_OnInterrupt;
        }

        private void SuspendEndUp()
        {
            _endUp.OnInterrupt -= endUp_OnInterrupt;
        }

        private void SuspendEndDown()
        {
            _endDown.OnInterrupt -= endDown_OnInterrupt;
        }

        #endregion

        #region Properties

        /// <summary>
        /// port-gathered flag, which is checked, if motor is under current
        /// </summary>
        private bool MotorDriving
        {
            get
            {
                return _swDc.Read();
            }
            set
            {
                _swDc.Write(value);

                if (_swDc.Read())
                {
                    _stopwatch.Start();
                }
                else
                {
                    _stopwatch.Stop();
                    Logger.Debug("MotorDriving=false (" + _stopwatch.RuntimeInMilliseconds + " ms)");
                    UpdateMotorPosition();
                    _stopCounter++;
                }
            }
        }

        /// <summary>
        /// port-gathered flag, which is checked, for: //true --> MovingUp; false --> MovingDown;
        /// </summary>
        private bool MotorDirection
        {
            get
            {
                return _swDir.Read();
            }
            set
            {
                //switch the current, off, if DC is on
                var motorWasDriving = false;
                if (MotorDriving)
                {
                    _swDc.Write(false);
                    Thread.Sleep(100);
                    motorWasDriving = true;
                }

                _swDir.Write(value);
                Thread.Sleep(100);

                //switch the current, on, if DC was on
                if (motorWasDriving)
                {
                    _swDc.Write(true);
                }
            }
        }

        /// <summary>
        /// wrapper-property 4 logging
        /// </summary>
        private ILoggerAppender Logger
        {
            get
            {
                return mfLogger.mfLogger.Instance;
            }
        }

        #endregion

        #region IMotor
        public bool IsInitialized { get; private set; }

        public void Initialize()
        {
            InitConfigs();

            MoveUp();
            SuspendEndUp();

            ResumeEndSensorsAfterDelayAsync();
            Thread.Sleep(MotorConfig.Delay + 1000);//ensure, that position is out of end sensor suspension

            MoveDown();
            SuspendEndDown();

            //wait on end of initialization or timeout
            int _timeout = 2 * MotorConfig.Duration + 2 * MotorConfig.Delay;
            var i = 0;
            do
            {
                Thread.Sleep(MotorConfig.Wait);

                if (DrivingDirection == enumMotorDrivingDirection.EndDown)
                {
                    IsInitialized = true;
                    break;
                }

                i++;
            } while ((i * MotorConfig.Wait) < _timeout);

            if ((i * MotorConfig.Wait) >= _timeout)
            {
                Logger.Warning(GetType().Name + ": can't be initialized, timeout exceeded");
                return;
            }

            if (this.IsInitialized)
                Logger.Info(this.GetType().Name + " is initialized");
            else
                Logger.Error(this.GetType().Name + " is NOT initialized");

            OnNotifyPropertyChanged("IsInitialized", IsInitialized);

            _stopwatch.Reset();
            _stopCounter = 0;
            Position = 0;
        }

        private void InitConfigs()
        {
            if (!ConfigurationProvider.Instance.HasConfig)
                throw new OSTException("no config found, motor-init");

            MotorConfig = ConfigurationProvider.Instance.ConfigInstance.Motor;
            FrameConfig = ConfigurationProvider.Instance.ConfigInstance.Frame;

            //calculate way-time-coeffient
            _timeWayCoefficient = MotorConfig.Duration / FrameConfig.Stroke;
        }

        private void ResumeEndSensorsAfterDelayAsync()
        {
            var thread = new Thread(new ThreadStart(ResumeEndSensorsAfterDelay));
            thread.Start();
        }

        private void ResumeEndSensorsAfterDelay()
        {
            Thread.Sleep((MotorConfig.Duration / 2));
            ResumeEndDown();
            ResumeEndUp();
        }

        enumMotorDrivingDirection _drivingDirection = enumMotorDrivingDirection.None;
        public enumMotorDrivingDirection DrivingDirection
        {
            get
            {
                return _drivingDirection;
            }
            set
            {
                _drivingDirection = value;
            }
        }

        public int Position
        {
            get
            {
                return _currentPosition;
            }
            private set
            {
                _currentPosition = value;
            }
        }

        public MotorConfiguration MotorConfig { get; set; }

        public FrameConfiguration FrameConfig { get; set; }

        public bool Driving
        {
            get
            {
                return (_timer4Motor == null) ? true : false;
            }
        }

        public void MoveDown()
        {
            Logger.Debug("MoveDown() executed");

            _suspendedSensor = enumEndSensorType.EndUp;
            SuspendEndSensorsAsync();

            MotorDirection = false;
            MotorDriving = true;

            DrivingDirection = enumMotorDrivingDirection.MovingDown;
        }

        public void MoveUp()
        {
            Logger.Debug("MoveUp() executed");

            _suspendedSensor = enumEndSensorType.EndDown;
            SuspendEndSensorsAsync();

            MotorDirection = true;
            MotorDriving = true;

            DrivingDirection = enumMotorDrivingDirection.MovingUp;
        }

        public void Stop()
        {
            Logger.Debug("Stop() executed");

            MotorDriving = true;
        }

        public void Move2MotorPositionAsync(int motorTarget)
        {
            AssertIfMotorTargetIsInvalid(motorTarget);

            if (MotorDriving)
            {
                Logger.Warning("Move2MotorPositionAsync already invoked");
                return;
            }
            var normedTarget = System.Math.Abs(motorTarget - Position);
            if (normedTarget <= 2)
            {
                Logger.Warning("Move2MotorPositionAsync(motorTarget=" + Position
                    + ") already reached or increment too small " + normedTarget + " mm");
                OnMotorTargetReached(Position, motorTarget, false);
                return;
            }

            //calculate drive time 4 target position
            int targetRuntimeMS = MotorCalculator.CalculateRuntime4TargetPosition(motorTarget
                                                , Position
                                                , _timeWayCoefficient
                                                , DrivingDirection
                                                , MotorConfig.OverrunThr
                                                , MotorConfig.Overrun
                                                , _stopCounter);
            targetRuntimeMS -= 5;//switch offset, ~5 ms in debug mode

            if (targetRuntimeMS < 10)
            {
                Logger.Warning(targetRuntimeMS + " ms is to small to drive");
                return;
            }

            var targetDirectionState = enumMotorDrivingDirection.None;

            //set up direction
            //State = true --> MovingUp; State = false --> MovingDown;
            if (motorTarget > Position)
            {
                MoveUp();
            }
            else if (motorTarget <= Position)
            {
                MoveDown();
            }
            else
            {
                Logger.Warning("Move2MotorPositionAsync DrivingDirection is not valid");
                return;
            }

            if (motorTarget <= 0)
            {
                targetDirectionState = enumMotorDrivingDirection.EndDown;
            }
            else if (motorTarget >= FrameConfig.Stroke)
            {
                targetDirectionState = enumMotorDrivingDirection.EndUp;
            }

            //add start-preload time
            targetRuntimeMS += MotorConfig.Preload;

            //init&start (immediatly) timer 
            _timer4Motor = new Timer(_timer4Motor_Tick
                , new MotorTarget() { TargetPos = motorTarget, TargetDrivingDirection = targetDirectionState }
                , targetRuntimeMS
                , 0);//, startOffset = 0 --> start immediatly

        }

        private void AssertIfMotorTargetIsInvalid(int motorTarget)
        {
            if (motorTarget > FrameConfig.Stroke)
                throw new OSTException("Move2MotorPositionAsync motorTarget=" + motorTarget + " is greater then motor-stroke=" + FrameConfig.Stroke);
            if (motorTarget < 0)
                throw new OSTException("Move2MotorPositionAsync motorTarget have to be positive");
        }

        private void _timer4Motor_Tick(object state)
        {
            var targetPos = state as MotorTarget;

            //check & wait on targetDirState
            int timeout = MotorConfig.Duration + MotorConfig.Delay;
            var i = 0;
            do
            {
                if (DrivingDirection == targetPos.TargetDrivingDirection)
                {
                    break;
                }
                Thread.Sleep(MotorConfig.Wait);

                i++;
            } while ((i * MotorConfig.Wait) < timeout);

            if ((i * MotorConfig.Wait) > timeout)
            {
                Logger.Warning(targetPos + " timed out");
                OnMotorTargetReached(Position, targetPos.TargetPos, true);

                _timer4Motor.Dispose();
                _timer4Motor = null;
                return;
            }

            MotorDriving = false;

            _timer4Motor.Dispose();
            _timer4Motor = null;

            Logger.Info("MotorTargetReached(calcPos=" + Position
                + ", targetPos=" + targetPos.TargetPos + ", timedOut=" + false + ")");

            OnMotorTargetReached(Position, targetPos.TargetPos, false);
        }

        public event delMotorTargetReached MotorTargetReached;

        private void OnMotorTargetReached(int motorCalculatedPosition, int motorTargetPosition, bool timedOut)
        {
            if (MotorTargetReached != null)
            {
                MotorTargetReached(motorCalculatedPosition, motorTargetPosition, timedOut);
            }
        }

        public void Calibrate()
        {
            //TODO: Test the Endstop-calibration

            //(re-)initialize motor --> drive down
            _stopwatch.Reset();

            if (DrivingDirection != enumMotorDrivingDirection.EndDown)
                Initialize();

            if (DrivingDirection != enumMotorDrivingDirection.EndDown)
                throw new OSTException("motor initializable for calibration");

            //drive to EndUp-Sensor --> max. MotorStroke
            MoveUp();
            _stopwatch.Start();

            //TODO: calibrationtimeout = motor.MotorConfig.MoveDuration*2
            //wait until the target is reached
            do
            { }
            while (DrivingDirection != enumMotorDrivingDirection.EndUp);

            _stopwatch.Stop();

            var msg = "calibration-result:" + _stopwatch.RuntimeInMilliseconds;
            Logger.Debug(msg);

            //apply the new calibration
            ConfigurationProvider.Instance.ConfigInstance.Motor.Duration = _stopwatch.RuntimeInMilliseconds;
            MotorConfig.Duration = _stopwatch.RuntimeInMilliseconds;

            //save the updated config 
            ConfigurationProvider.Instance.SaveConfiguration();

            //(re-)initialize the motor
            Initialize();
        }

        #endregion

        #region INotifyPropertyChanged

        /// <summary>
        /// notifies clients over changes in puplic properties
        /// </summary>
        public event PropertyChangedEventHandler PropertyChanged;

        protected void OnNotifyPropertyChanged(String propertyName, object newValue)
        {

            if (PropertyChanged != null && !propertyName.IsNullOrEmpty())
            {
                PropertyChanged(this, new PropertyChangedEventArgs(propertyName, newValue));
            }
        }

        #endregion

        #region Nested Types

        private class MotorTarget
        {

            public enumMotorDrivingDirection TargetDrivingDirection { get; set; }

            /// <summary>
            /// absolute position
            /// </summary>
            public int TargetPos { get; set; }

            public override string ToString()
            {
                return "TargetDir: " + enumMotorDrivingDirectionTranslator.Translate(TargetDrivingDirection)
                        + "| TargetPos: " + TargetPos;
            }
        }

        #endregion

    }
}
